#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include "livox_ros_driver2/msg/custom_msg.hpp"

rmw_qos_profile_t qos_profile_lidar{RMW_QOS_POLICY_HISTORY_KEEP_LAST,
                                    5,
                                    RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
                                    RMW_QOS_POLICY_DURABILITY_VOLATILE,
                                    RMW_QOS_DEADLINE_DEFAULT,
                                    RMW_QOS_LIFESPAN_DEFAULT,
                                    RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
                                    RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
                                    false};

auto qos_lidar =
    rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_lidar.history, qos_profile_lidar.depth), qos_profile_lidar);

// 接受LIVOX-MID360点云数据，并发布sensor_msgs::msg::PointCloud2点云格式的数据
// 相当于消息格式转换
class Mid360Talker : public rclcpp::Node {
public:
    Mid360Talker() : Node("livox_mid360") {
        auto lidarOpt           = rclcpp::SubscriptionOptions();
        callback_group_lidar_   = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
        lidarOpt.callback_group = callback_group_lidar_;
        sub_laser_cloud_        = create_subscription<livox_ros_driver2::msg::CustomMsg>(
            "livox/lidar", qos_lidar, std::bind(&Mid360Talker::cloudHandler, this, std::placeholders::_1), lidarOpt);

        pub_laser_cloud_ = create_publisher<sensor_msgs::msg::PointCloud2>("points_raw", 1);
    }

private:
    void cloudHandler(const livox_ros_driver2::msg::CustomMsg::SharedPtr cloud_msg) {
        sensor_msgs::msg::PointCloud2 cloud;
        CustomMsg2PointCloud2(cloud_msg, cloud);
        pub_laser_cloud_->publish(cloud);
    }

    // 将livox-mid360的CustomMsg消息转换成lio-sam需要的点云数据类型
    // TODO::后期修改源码，保证点云数据的通用性
    void CustomMsg2PointCloud2(const livox_ros_driver2::msg::CustomMsg::SharedPtr cloud_in,
                               sensor_msgs::msg::PointCloud2                     &cloud_out) {
        // header
        cloud_out.header.stamp    = cloud_in->header.stamp;
        cloud_out.header.frame_id = cloud_in->header.frame_id;
        cloud_out.height          = 1;
        cloud_out.width           = cloud_in->points.size();

        // 定义fields字段信息
        sensor_msgs::msg::PointField field;
        // x
        field.name     = "x";
        field.offset   = 0;
        field.datatype = sensor_msgs::msg::PointField::FLOAT32;
        field.count    = 1;
        cloud_out.fields.emplace_back(field);

        // y
        field.name     = "y";
        field.offset   = 4;
        field.datatype = sensor_msgs::msg::PointField::FLOAT32;
        field.count    = 1;
        cloud_out.fields.emplace_back(field);

        // z
        field.name     = "z";
        field.offset   = 8;
        field.datatype = sensor_msgs::msg::PointField::FLOAT32;
        field.count    = 1;
        cloud_out.fields.emplace_back(field);

        // intensity
        field.name     = "intensity";
        field.offset   = 12;
        field.datatype = sensor_msgs::msg::PointField::FLOAT32;
        field.count    = 1;
        cloud_out.fields.emplace_back(field);

        // ring
        field.name     = "ring";
        field.offset   = 16;
        field.datatype = sensor_msgs::msg::PointField::UINT16;
        field.count    = 1;
        cloud_out.fields.emplace_back(field);

        // time
        field.name     = "time";
        field.offset   = 18;
        field.datatype = sensor_msgs::msg::PointField::FLOAT32;
        field.count    = 1;
        cloud_out.fields.emplace_back(field);

        // 计算点云数据的总大小
        cloud_out.point_step = 22;  // 每个点占22个字节
        cloud_out.row_step   = cloud_out.point_step * cloud_out.width;

        // 填充点云数据
        cloud_out.data.resize(cloud_out.row_step);
        for (int i = 0; i < cloud_in->points.size(); i++) {
            const auto &point  = cloud_in->points[i];
            size_t      offset = i * cloud_out.point_step;
            memcpy(&cloud_out.data[offset + 0], &point.x, sizeof(float));  // 填充x
            memcpy(&cloud_out.data[offset + 4], &point.y, sizeof(float));  // 填充y
            memcpy(&cloud_out.data[offset + 8], &point.z, sizeof(float));  // 填充z

            // 填充intensity
            float intensity = static_cast<float>(point.reflectivity);
            memcpy(&cloud_out.data[offset + 12], &intensity, sizeof(float));

            // 填充ring
            uint16_t ring = static_cast<uint16_t>(point.line);
            memcpy(&cloud_out.data[offset + 16], &ring, sizeof(uint16_t));

            // 填充time
            float time = static_cast<float>(point.offset_time) * 1e-9;
            memcpy(&cloud_out.data[offset + 18], &time, sizeof(float));
        }

        // PrintPointCloud2(cloud_out);
    }

    void PrintPointCloud2(const sensor_msgs::msg::PointCloud2 &cloud_msg) {
        // header
        std::cout << "stamp :" << rclcpp::Time(cloud_msg.header.stamp).seconds() << std::endl;
        std::cout << "frame_id:" << cloud_msg.header.frame_id << std::endl;
        std::cout << "height: " << cloud_msg.height << std::endl;
        std::cout << "width: " << cloud_msg.width << std::endl;
        std::cout << "point_step: " << cloud_msg.point_step << std::endl;
        std::cout << "row_step: " << cloud_msg.row_step << std::endl;
        std::cout << "is_bigendian: " << (cloud_msg.is_bigendian ? "true" : "false") << std::endl;
        std::cout << "is_dense: " << (cloud_msg.is_dense ? "true" : "false") << std::endl;
        for (const auto &field : cloud_msg.fields) {
            for (size_t i = 0; i < cloud_msg.height * cloud_msg.width; ++i) {
                // 根据字段偏移量读取数据
                const uint8_t *data_ptr = &cloud_msg.data[i * cloud_msg.point_step];  // 获取每个点的数据指针

                // 读取 x, y, z 数据
                float    x         = *reinterpret_cast<const float *>(data_ptr + cloud_msg.fields[0].offset);
                float    y         = *reinterpret_cast<const float *>(data_ptr + cloud_msg.fields[1].offset);
                float    z         = *reinterpret_cast<const float *>(data_ptr + cloud_msg.fields[2].offset);
                float    intensity = *reinterpret_cast<const float *>(data_ptr + cloud_msg.fields[3].offset);
                uint16_t ring      = *reinterpret_cast<const uint16_t *>(data_ptr + cloud_msg.fields[4].offset);
                float    time      = *reinterpret_cast<const float *>(data_ptr + cloud_msg.fields[5].offset);

                std::cout << "x:" << x << std::endl;
                std::cout << "y:" << y << std::endl;
                std::cout << "z:" << z << std::endl;
                std::cout << "intensity:" << intensity << std::endl;
                std::cout << "ring:" << (int) ring << std::endl;
                std::cout << "time:" << time << std::endl;
                std::cout << std::endl;
            }
        }
    }

    void PrintCustmMsg(const livox_ros_driver2::msg::CustomMsg &cloud_msg) {
        // header
        std::cout << "frame_id:" << cloud_msg.header.frame_id << std::endl;
        std::cout << "stamp:" << cloud_msg.timebase << std::endl;  // 单位::ns

        // Points
        for (const auto &p : cloud_msg.points) {
            std::cout << "x:" << p.x << std::endl;
            std::cout << "y:" << p.y << std::endl;
            std::cout << "z:" << p.z << std::endl;
            std::cout << "intensity:" << (int) p.reflectivity << std::endl;
            std::cout << "tag:" << (int) p.tag << std::endl;
            std::cout << "line:" << (int) p.line << std::endl;
            std::cout << "offset_time:" << p.offset_time << std::endl;
        }
    }

    // 订阅livox-MID360点云数据
    rclcpp::Subscription<livox_ros_driver2::msg::CustomMsg>::SharedPtr sub_laser_cloud_;
    rclcpp::CallbackGroup::SharedPtr                                   callback_group_lidar_;

    // 发布通用点云数据
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_laser_cloud_;
};

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);

    rclcpp::NodeOptions options;
    options.use_intra_process_comms(true);
    rclcpp::executors::SingleThreadedExecutor exec;

    auto mid360_talker = std::make_shared<Mid360Talker>();
    exec.add_node(mid360_talker);

    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "\033[1;32m----> MID360 Talker Started.\033[0m");

    exec.spin();

    rclcpp::shutdown();
    return 0;
}